/*
 * CommProtocol_Commands.cpp
 *
 *  Created on: Jan 31, 2014
 *      Author: ppks
 */

#include "common.h"
#include "CommProtocol.h"
#include "CommProtocol_Commands.h"
#include "Serial.h"
#include "i2c.h"
#include "FlightController_Common.h"
#include "EEPROM.h"
#include "main.h"
#include "MotorControl.h"
#include "RemoteControl.h"
#include "GPS_common.h"
#include "Debug.h"

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/


/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/


/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/

/*******************************************************************************
* Function Name: CommProtocol_GetIdentInfoCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetIdentInfoCMD(void)
{
    struct {
        uint8_t version;
        uint8_t type;
        uint8_t msp_v;
        uint32_t capabilities;
    } id;

    /* Fill the structure with information */
    id.version      = VERSION;
    id.type         = MULTITYPE;
    id.msp_v        = MSP_VERSION;
    id.capabilities = NAV_CAP << 4 | EXTAUX_CAP << 5 | ((uint32_t)NAVI_VERSION<<28);          /* Navi version is stored in the upper four bits */

    /* Send the data to the host */
    CommProtocol_SendBlock((uint8_t*)&id, 7);
}

/*******************************************************************************
* Function Name: CommProtocol_GetStatusCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetStatusCMD(void)
{
    struct {
       uint16_t cycleTime;
       uint16_t i2c_errors_count;
       uint16_t sensorConfig;
       uint32_t modesFlag;
       uint8_t settings;
    } status;

    /* Fill the structure with status information */
    status.cycleTime = loopCycleTime;
    status.i2c_errors_count = i2c_errors_count;
    status.sensorConfig = ACC_INSTALLED |
                          (BARO_INSTALLED << 1) |
                          (MAG_INSTALLED << 2) |
                          (GPS_INSTALLED << 3) |
                          (SONAR_INSTALLED << 4);

    status.modesFlag = FlightController_GetActiveModesFlag();
    status.settings = ConfigInfo.activeSet;

    /* Send the data to the host */
    CommProtocol_SendBlock((uint8_t*)&status, 11);
}

/*******************************************************************************
* Function Name: CommProtocol_GetIMURawDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetIMURawDataCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&imuData, 18);
}

/*******************************************************************************
* Function Name: CommProtocol_SendMotorData
********************************************************************************
* Summary:
*  Sends the data of motor control values
*
*******************************************************************************/
void CommProtocol_GetMotorDataCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&motorControlData, 16);
}

/*******************************************************************************
* Function Name: CommProtocol_SendMotorData
********************************************************************************
* Summary:
*  Sends the data of motor control values
*
*******************************************************************************/
void CommProtocol_GetServoDataCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&servoControlData, 16);
}

/*******************************************************************************
* Function Name: CommProtocol_GetRCDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetRCRawDataCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&rcData, RC_CHANNELS_NUM * 2);
}

/*******************************************************************************
* Function Name: CommProtocol_GetGPSRawDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetGPSRawDataCMD(void)
{
    CommProtocol_HeaderReply(16);

    CommProtocol_AppendUint8(globalFlags.GPS_FIX);
    CommProtocol_AppendUint8(GPS_NumSat);
    CommProtocol_AppendUint32(GPS_CurrentCoord[LAT]);
    CommProtocol_AppendUint32(GPS_CurrentCoord[LON]);
    CommProtocol_AppendUint16(GPS_Altitude);
    CommProtocol_AppendUint16(GPS_Speed);
    CommProtocol_AppendUint16(GPS_GroundCourse);
}

/*******************************************************************************
* Function Name: CommProtocol_GetGPSComputedDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetGPSComputedDataCMD(void)
{
    CommProtocol_HeaderReply(5);

    CommProtocol_AppendUint16(GPS_DistanceToHome);
    CommProtocol_AppendUint16(GPS_DirectionToHome);
    CommProtocol_AppendUint8(GPS_Update & 1);
}

/*******************************************************************************
* Function Name: CommProtocol_GetAttitudeCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetAttitudeCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&att, 6);
}

/*******************************************************************************
* Function Name: CommProtocol_GetAltitudeCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetAltitudeCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&alt, 6);
}

/*******************************************************************************
* Function Name: CommProtocol_GetAnalogData
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetAnalogData(void)
{
	CommProtocol_SendBlock((uint8_t*)&analogData, 7);
}

/*******************************************************************************
* Function Name: CommProtocol_GetRCTuningCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetRCTuningCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&FC_Config.RC.rcRate8, 7);
}

/*******************************************************************************
* Function Name: CommProtocol_GetPIDSettingsCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetPIDSettingsCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&FC_Config.PID[0].P8, 3*PIDITEMS);
}

/*******************************************************************************
* Function Name: CommProtocol_GetBoxConfigCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetBoxConfigCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&FC_Config.BOX.activate[0], AUX_BOX_ITEM_SIZE * CHECKBOXITEMS);
}

/*******************************************************************************
* Function Name: CommProtocol_GetMiscDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetMiscDataCMD(void)
{
    struct {
        uint16_t a,b,c,d,e,f;
        uint32_t g;
        uint16_t h;
        uint8_t  i,j,k,l;
    } misc;

        misc.a = 0;/*intPowerTrigger1;*/
        misc.b = FC_Config.MISC.minthrottle;
        misc.c = MAXTHROTTLE;
        misc.d = MOTOR_MINCOMMAND;
#ifdef FAILSAFE
        misc.e = FC_Config.MISC.failsafe_throttle;
#else /* FAILSAFE */
        misc.e = 0;
#endif /* FAILSAFE */
#ifdef LOG_PERMANENT
        misc.f = plog.arm;
        misc.g = plog.lifetime + (plog.armed_time / 1000000); // <- computation must be moved outside from serial
#else /* LOG_PERMANENT */
        misc.f = 0; misc.g =0;
#endif /* LOG_PERMANENT */
#if MAG_INSTALLED
        misc.h = FC_Config.MISC.mag_declination;
#else /* MAG_INSTALLED */
        misc.h = 0;
#endif /* MAG_INSTALLED */
#ifdef VBAT_ENABLED
        misc.i = FC_Config.MISC.vbatscale;
        misc.j = FC_Config.MISC.vbatlevel_warn1;
        misc.k = FC_Config.MISC.vbatlevel_warn2;
        misc.l = FC_Config.MISC.vbatlevel_crit;
#else /* VBAT_ENABLED */
        misc.i = 0;
        misc.j = 0;
        misc.k = 0;
        misc.l = 0;
#endif /* VBAT_ENABLED */

        CommProtocol_SendBlock((uint8_t*)&misc, 22);
}

/*******************************************************************************
* Function Name: CommProtocol_GetMotorPinsConfigCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetMotorPinsConfigCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&PWM_PIN, 8);
}

/*******************************************************************************
* Function Name: CommProtocol_GetBoxItemNamesCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetBoxItemNamesCMD(void)
{
    CommProtocol_SendNames(BOXnames);
}

/*******************************************************************************
* Function Name: CommProtocol_GetPIDNames
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetPIDNames(void)
{
    CommProtocol_SendNames(PIDnames);
}

/*******************************************************************************
* Function Name: CommProtocol_GetWaypointsDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetWaypointsDataCMD(void)
{
    /* The structure is changed to match host command */
    WaypointRecord_t reportPacket;

    uint8_t WPIndex;
    uint8_t missionFlag;

    /* Get the WP number */
    WPIndex = CommProtocol_ReadUint8();

    if ((WPIndex > 0) || (WPIndex < 255))
    {
        /* Read mission from EEPROM */
        if (NAV_state == NAV_STATE_NONE)
        {
            if (!EEPROM_ReadWPRecord(WPIndex))
            {
                /* Mark that mission is not read correctly */
                missionFlag = MISSION_FLAG_CRC_ERROR;
            }
        }
        else
        {
            missionFlag = MISSION_FLAG_NAV_IN_PROG;
        }
    }

    /* Write the structure to the report packet */
    memcpy(&reportPacket, &WaypointRecord, sizeof(WaypointRecord_t));

    /* Update the rest of the flags based on the conditions */
    reportPacket.number = WPIndex;

    if (WPIndex == 0)
    {
        /* Get HOME coordinates */
        reportPacket.pos[LAT] = GPS_HomeCoord[LAT];
        reportPacket.pos[LON] = GPS_HomeCoord[LON];
        missionFlag = MISSION_FLAG_HOME;
    }
    else if (WPIndex == 255)
    {
        /* Get poshold coordinates */
        reportPacket.pos[LAT] = GPS_HoldCoord[LAT];
        reportPacket.pos[LON] = GPS_HoldCoord[LON];
        missionFlag = MISSION_FLAG_HOLD;
    }

    if (missionFlag == MISSION_FLAG_NAV_IN_PROG)
    {
        reportPacket.action = MISSION_ACTION_NONE;
        reportPacket.pos[LAT] = GPS_HomeCoord[LAT];
        reportPacket.pos[LON] = GPS_HomeCoord[LON];
    }
    reportPacket.flag = missionFlag;

    /* Send Structure to Host */
    CommProtocol_SendBlock((uint8_t*)&reportPacket, sizeof(reportPacket) - 1);
}

/*******************************************************************************
* Function Name: CommProtocol_GetBoxIdsCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetBoxIdsCMD(void)
{
    CommProtocol_HeaderReply(CHECKBOXITEMS);

    for (uint8_t i = 0; i < CHECKBOXITEMS; i++)
    {
        CommProtocol_AppendUint8(pgm_read_byte(&(BOXids[i])));
    }
}

/*******************************************************************************
* Function Name: CommProtocol_GetServoConfigCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetServoConfigCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&servoControlData, 16);
}

/*******************************************************************************
* Function Name: CommProtocol_GetGPSNavStatusCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetGPSNavStatusCMD(void)
{
    CommProtocol_HeaderReply(7);
    CommProtocol_AppendUint8(globalFlags.GPS_MODE);
    CommProtocol_AppendUint8(NAV_state);
    CommProtocol_AppendUint8(WaypointRecord.action);
    CommProtocol_AppendUint8(WaypointRecord.number);
    CommProtocol_AppendUint8(NAV_error);
    CommProtocol_AppendUint16( (int16_t)(GPS_TargetBearing / 100));
}

/*******************************************************************************
* Function Name: CommProtocol_GetGPSConfigCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetGPSConfigCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&GPS_Config, sizeof(GPS_Config));
}

/*******************************************************************************
* Function Name: CommProtocol_SetRCRawDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetRCRawDataCMD(void)
{
    CommProtocol_ReadBlock((uint8_t*) &rcSerial, 2 * RC_CHANNELS_NUM);

    /* 1st transition - initialize count to 50 */
    rcSerialCount = 50;
}

/*******************************************************************************
* Function Name: CommProtocol_SetGPSRawDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetGPSRawDataCMD(void)
{
    /* Read the data from the buffer */
    globalFlags.GPS_FIX = CommProtocol_ReadUint8();
    GPS_NumSat = CommProtocol_ReadUint8();
    GPS_CurrentCoord[LAT] = CommProtocol_ReadUint32();
    GPS_CurrentCoord[LON] = CommProtocol_ReadUint32();
    GPS_Altitude = CommProtocol_ReadUint16();
    GPS_Speed = CommProtocol_ReadUint16();
    GPS_Update |= 2; /* New data signalization to GPS functions */

    /* Send reply to the host */
    CommProtocol_HeaderReply(0);
}

/*******************************************************************************
* Function Name: CommProtocol_SetPIDSettingsCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetPIDSettingsCMD(void)
{
    CommProtocol_ReadBlock((uint8_t*)&FC_Config.PID[0], 3*PIDITEMS);
#if GPS_INSTALLED
    /* Set GPS PIDs */
    GPS_SetPIDs();
#endif /* GPS_INSTALLED */
}

/*******************************************************************************
* Function Name: CommProtocol_SetBoxConfigCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetBoxConfigCMD(void)
{
    CommProtocol_ReadBlock((uint8_t*)&FC_Config.BOX.activate[0], AUX_BOX_ITEM_SIZE * CHECKBOXITEMS);
}

/*******************************************************************************
* Function Name: CommProtocol_SetRCTuningCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetRCTuningCMD(void)
{
    CommProtocol_ReadBlock((uint8_t*)&FC_Config.RC, 7);
}

/*******************************************************************************
* Function Name: CommProtocol_RequestAccCalibrationCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_RequestAccCalibrationCMD(void)
{
    /* Set number of calibration cycles to default if motors are not armed */
    if (!globalFlags.ARMED)
    {
        calibration.AccCycles = ACC_CALIBRATION_CYCLES_DEFAULT;
    }

    /* Send reply to the host */
    CommProtocol_HeaderReply(0);
}

/*******************************************************************************
* Function Name: CommProtocol_RequestMagCalibrationCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_RequestMagCalibrationCMD(void)
{
    /* Set number of calibration cycles to default if motors are not armed */
    if (!globalFlags.ARMED)
    {
        calibration.MagCycles = MAG_CALIBRATION_CYCLES_DEFAULT;
    }

    /* Send reply to the host */
    CommProtocol_HeaderReply(0);
}

/*******************************************************************************
* Function Name: CommProtocol_SetMiscDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetMiscDataCMD(void)
{
    struct {
       uint16_t a,b,c,d,e,f;
       uint32_t g;
       uint16_t h;
       uint8_t  i,j,k,l;
    } set_misc;

    CommProtocol_ReadBlock((uint8_t*)&set_misc, 22);

#if defined(POWERMETER)
    FC_Config.MISC.powerTrigger1 = set_misc.a / PLEVELSCALE;
#endif /* defined(POWERMETER) */
    FC_Config.MISC.minthrottle = set_misc.b;
#ifdef FAILSAFE
    FC_Config.MISC.failsafe_throttle = set_misc.e;
#endif /* FAILSAFE */
#if MAG_INSTALLED
    FC_Config.MISC.mag_declination = set_misc.h;
#endif /* MAG_INSTALLED */
#if defined(VBAT_ENABLED)
    FC_Config.MISC.vbatscale        = set_misc.i;
    FC_Config.MISC.vbatlevel_warn1  = set_misc.j;
    FC_Config.MISC.vbatlevel_warn2  = set_misc.k;
    FC_Config.MISC.vbatlevel_crit   = set_misc.l;
#endif /* defined(VBAT_ENABLED) */
}

/*******************************************************************************
* Function Name: CommProtocol_ResetSystemConfigCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_ResetSystemConfigCMD(void)
{
    /* Check if the motors are not armed then load defaults */
    if (!globalFlags.ARMED)
    {
        FC_LoadConfigDefaults();

        /* write defaults to EEPROM */
        EEPROM_WriteParams(ConfigInfo.activeSet, 1);
    }

    /* Read Back the data to initialize all the variables properly */
    FlightController_LoadConfiguration();

    /* Send reply to the host */
    CommProtocol_HeaderReply(0);
}

/*******************************************************************************
* Function Name: CommProtocol_GetServoConfigCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetWaypointsCMD(void)
{
    WaypointRecord_t receivedPacket;

    /* Read incoming WP structure without checksum field */
    CommProtocol_ReadBlock((uint8_t*)&receivedPacket, sizeof(receivedPacket) - 1);


    if ((NAV_state == NAV_STATE_HOLD_INFINIT) && (receivedPacket.number == 255))
    {
        /* Special case - during stable poshold we allow change the hold position */
        receivedPacket.action = MISSION_HOLD_UNLIM;

        memcpy(&WaypointRecord, &receivedPacket, sizeof(receivedPacket) - 1);

        if (WaypointRecord.altitude != 0)
        {
            GPS_SetNewAltitude(WaypointRecord.altitude);
        }

        GPS_SetNextWP(WaypointRecord.pos, GPS_CurrentCoord);

        if ((GPS_DistanceToWP / 100) >= GPS_Config.safe_wp_distance)
        {
            NAV_state = NAV_STATE_NONE;
        }
        else
        {
            NAV_state = NAV_STATE_WP_ENROUTE;
        }
        return;
    }

    if (NAV_state == NAV_STATE_NONE)
    {
        memcpy(&WaypointRecord, &receivedPacket, sizeof(receivedPacket) - 1);

        if (WaypointRecord.number == 0)                                            //Set new Home position
        {
            GPS_HomeCoord[LAT] = WaypointRecord.pos[LAT];
            GPS_HomeCoord[LON] = WaypointRecord.pos[LON];
        }

        if ((WaypointRecord.number > 0) && (WaypointRecord.number < 255))
        {
            /* Not home and not poshold, we are free to store it in the EEPROM */
            if (WaypointRecord.number <= EEPROM_GetMaxWaypointRecords())
            {
                /* Do not thrash the EEPROM with invalid WP number */
                EEPROM_WriteWPRecord();
            }
        }
    }
    else
    {
        /* The Nav state is not zero, so navigation is in progress, silently ignore SET_WP command) */
    }
}

/*******************************************************************************
* Function Name: CommProtocol_SelectSettingsSetCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SelectSettingsSetCMD(void)
{
#ifdef MULTIPLE_CONFIGURATION_PROFILES
    if (!globalFlags.ARMED)
    {
        CommProtocol_ReadBlock((uint8_t*)&ConfigInfo.activeSet, 1);
        if (ConfigInfo.activeSet > 2)
        {
            ConfigInfo.activeSet = 0;
        }
        /* Update Config Information in the EEPROM */
        EEPROM_UpdateConfigInfo();

        /* Load Config and Calc RC LUTs */
        FlightController_LoadConfiguration();
    }
    else
#endif /* MULTIPLE_CONFIGURATION_PROFILES */
    {
        /* Send reply to the host */
        CommProtocol_HeaderReply(0);
    }
}

/*******************************************************************************
* Function Name: CommProtocol_SetHeadingCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetHeadingCMD(void)
{
    CommProtocol_ReadBlock((uint8_t*)&magHold, 2);
}

/*******************************************************************************
* Function Name: CommProtocol_SetServoConfigCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetServoConfigCMD(void)
{
    CommProtocol_ReadBlock((uint8_t*)&FC_Config.MISC.servoConf, 56);
}

/*******************************************************************************
* Function Name: CommProtocol_SetMotorDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetMotorDataCMD(void)
{
    CommProtocol_ReadBlock((uint8_t*)&motorControlData, 16);
}

/*******************************************************************************
* Function Name: CommProtocol_SetGPSConfigCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_SetGPSConfigCMD(void)
{
    CommProtocol_ReadBlock((uint8_t*)&GPS_Config, sizeof(GPS_Config));
}

/*******************************************************************************
* Function Name: CommProtocol_BindCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_BindCMD(void)
{
    /* TODO */
}

/*******************************************************************************
* Function Name: CommProtocol_EEPROMWriteCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_EEPROMWriteCMD(void)
{
    /* Write Params to EEPROM */
    EEPROM_UpdateParams(ConfigInfo.activeSet, 0);

    /* Read back the parameters to ensure they were written correctly.
     * If the checksum doesn't match the defaults will be loaded */
    EEPROM_ReadParams(ConfigInfo.activeSet);

    /* Send reply to the host */
    CommProtocol_HeaderReply(0);
}



/* Debug commands */
/*******************************************************************************
* Function Name: CommProtocol_GetDebugMessageCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetDebugMessageCMD(void)
{
    uint8_t size = Debug_GetBuffSizeAvailable();

    /* Limit message size to 16 */
    if (size > 16)
    {
        size = 16;
    }

    /* Send header reply with actual size of the message */
    CommProtocol_HeaderReply(size);

    /* Send the debug message */
    Debug_SendMessage(size);
}

/*******************************************************************************
* Function Name: CommProtocol_GetDebugDataCMD
********************************************************************************
* Summary:
*
*
*******************************************************************************/
void CommProtocol_GetDebugDataCMD(void)
{
    CommProtocol_SendBlock((uint8_t*)&debugBuffer, 8);
}



